import numpy as np
import cv2 as cv
from matplotlib import pyplot as plt
img1 = cv.imread('left01.jpg',0)  #queryimage # left image
img2 = cv.imread('right01.jpg',0) #trainimage # right image
sift = cv.xfeatures2d.SIFT_create()
# find the keypoints and descriptors with SIFT
kp1, des1 = sift.detectAndCompute(img1,None)
kp2, des2 = sift.detectAndCompute(img2,None)
# FLANN parameters
FLANN_INDEX_KDTREE = 1
index_params = dict(algorithm = FLANN_INDEX_KDTREE, trees = 5)
search_params = dict(checks=50)
flann = cv.FlannBasedMatcher(index_params,search_params)
matches = flann.knnMatch(des1,des2,k=2)
good = []
pts1 = []
pts2 = []
# ratio test as per Lowe's paper
for i,(m,n) in enumerate(matches):
    if m.distance < 0.8*n.distance:
        good.append(m)
        pts2.append(kp2[m.trainIdx].pt)
        pts1.append(kp1[m.queryIdx].pt)
pts2=np.float32(pts2)
pts1=np.float32(pts1)
#找本质矩阵
E, mask = cv.findEssentialMat(pts1, pts2, focal=1.0, pp=(5., 5.), method=cv.RANSAC, prob=0.999, threshold=3.0)
points, R, t, mask = cv.recoverPose(E, pts1, pts2)

#打印旋转和平移矩阵
print("R=",R)
print("t=",t)